#ifndef MOTOR_ENGINE_H
#define MOTOR_ENGINE_H

#include <SMC.h>
#include "TB67S109A.h"
#include "linkage_steps_factory.h"
#include <Scheduler.h>
#include <stdint.h>

enum class LinkageMotionState :uint8_t {Inactive,Start,Ready,Running,Stop,WaitingDelay,StepOnceEnd,Complete};

enum class CycleMotionState :uint8_t {Inactive,Start,Restart,Ready,Running,Complete};

typedef struct
{
    LinkageMotionState state;
    uint8_t current_linkage_steps;  
    uint8_t total_linkage_steps;  //todo
    linkage_steps_factory::linkage_steps_setup_t::memory_node_t* node;
    unsigned long delay_100ms;//todo
    unsigned long last_millis;
    long target_position_in_steps;
    uint16_t current_stage_delay_100ms;
    
}linkage_motion_control_t;

typedef struct
{
    CycleMotionState state;
    uint16_t total_cycle_count;
    uint16_t current_cycle_count;
}cycle_motion_control_t;

typedef struct
{
    uint8_t id;
    bool manual;
    linkage_motion_control_t linkage_motion_control;
    cycle_motion_control_t  cycle_motion_control;
}stepper_compose_motion_t;

typedef struct{
    union{
        uint16_t value;
        uint8_t data[2];
    };
}stepper_compose_data_t;



class SMCEngine : public Scheduler
{
public:
    enum class MotorName{M1,M2,M3,M4};
    SMCEngine(SMC& mcs,DigitalOut& interruptPin);
    ~SMCEngine();

    void begin();
    void reset(stepper_compose_motion_t& mcc,uint8_t id);
    // bool init_m1_mcc(uint16_t cycle_count,uint8_t total_stage_num,uint8_t stage_offset_num);
    // bool init_m2_mcc(uint16_t cycle_count,uint8_t total_stage_num,uint8_t stage_offset_num);
    // bool init_m3_mcc(uint16_t cycle_count,uint8_t total_stage_num,uint8_t stage_offset_num);
    bool init_mcc(stepper_compose_motion_t& mcc);

    void m1_fire();
    void m2_fire();
    void m3_fire();

    void schedule() override;
    void update() override;
    void dispatch(uint8_t* data,size_t size) {}
    void cleanup()override;

    LinkageMotionState handle_linkage_steps(stepper_compose_motion_t& mcc);
    void handle_cycle_motion(stepper_compose_motion_t& mcc);
    
    void enable_control_all_drivers(bool m1_en,bool m2_en,bool m3_en);
    void enable_control_driver(MotorName name,bool en);

    void clear_interrupt();

    void set_state(uint8_t id,uint8_t value);

    uint8_t m1_current_total_linkage_steps()
    {
        return _m1_mcc.linkage_motion_control.current_linkage_steps;
    }
    int16_t m2_current_total_linkage_steps()
    {
        return _m2_mcc.linkage_motion_control.current_linkage_steps;
    }
    int16_t m3_current_total_linkage_steps()
    {
        return _m3_mcc.linkage_motion_control.current_linkage_steps;
    }

    uint8_t m1_current_cycle_count()
    {
        return _m1_mcc.cycle_motion_control.current_cycle_count;
    }
    int16_t m2_current_cycle_count()
    {
        return _m2_mcc.cycle_motion_control.current_cycle_count;
    }
    int16_t m3_current_cycle_count()
    {
        return _m3_mcc.cycle_motion_control.current_cycle_count;
    }
private:
    SMC& _smc;
    DigitalOut& _interruptPin;
    stepper_compose_motion_t _m1_mcc;
    stepper_compose_motion_t _m2_mcc;
    stepper_compose_motion_t _m3_mcc;
};

#endif